#include "../inc/pid.h"

#include <cmath>

// ----------------------- IncrementalPID -----------------------

// 构造函数，初始化 PID 参数
IncrementalPID::IncrementalPID(float Kp, float Ki, float Kd)
    : Kp(Kp), Ki(Ki), Kd(Kd), prev_error(0.0), prev_prev_error(0.0) {
}

// 计算增量式 PID 输出
int IncrementalPID::compute(int setpoint, int measured_value) {
    error = setpoint - measured_value;

    int increment = static_cast<int>(Kp * (error - prev_error) +
        Ki * error +
        Kd * (error - 2*prev_error + prev_prev_error));

    prev_prev_error = prev_error;
    prev_error = error;
    
    return increment;
}

// ----------------------- PositionPID -----------------------

// 构造函数，初始化 PD 参数（此处不使用积分项）
PositionPID::PositionPID(float Kp, float Kp2, float Kd, float Kd2,float Kp_circle, float Kp2_circle, float Kd_circle, float Kd2_circle)
    : Kp(Kp), Kp2(Kp2), Kd(Kd), Kd2(Kd2),Kp_circle(Kp_circle), Kp2_circle(Kp2_circle), Kd_circle(Kd_circle), Kd2_circle(Kd2_circle), prev_position(0.0f) {}

// 计算位置式 PD 输出
float PositionPID::compute(float position, int gyroz) {
    float output = static_cast<float>(position * Kp + std::abs(position) * position * Kp2 + (position - prev_position) * Kd + gyroz * Kd2);

    prev_position = position;

    return output;
}

float PositionPID::compute_fuzzy(float position,int gyroz) {
    float output = static_cast<float>(position * Kp_fuzzy + (position - prev_position) * Kd_fuzzy+gyroz * Kd2);


    prev_position = position;

    return output;
}

float PositionPID::compute_circle(float position, int gyroz) {
    float output = static_cast<float>(position * Kp_circle + std::abs(position) * position * Kp2_circle + (position - prev_position) * Kd_circle + gyroz * Kd2_circle);

    prev_position = position;

    return output;
}

